package simpleGeom;

import java.awt.geom.Line2D;

import processing.core.PMatrix3D;

public class MTrianglePreCalcDouble {
	static double SMALL_NUM = 0.00000001f;
	// 0 = disjoint (no intersect)
	// 1 = intersect in unique point I1
	// 2 = are in the same plane
	final static int DISJOINT = 0;
	final static int INTERSECT = 1;
	final static int SAME_PLANE = 2;

	MSegmentDouble l1;
	MSegmentDouble l2;
	MSegmentDouble l3;

	PMatrix3D m;
	double ux, uy, uz;
	double vx, vy, vz;
	double nx, ny, nz;
	double x1, y1, z1, x2, y2, z2, x3, y3, z3;
	double d0 = 0;// distanceToOrigin;
	double uu, uv, vv;
	double d;

	Vec3D a,b,c;
	 double sd;
	public MTrianglePreCalcDouble() {

	}

	public MTrianglePreCalcDouble(double x1, double y1, double z1, double x2, double y2,
			double z2, double x3, double y3, double z3) {
		this.x1 = x1;
		this.y1 = y1;
		this.z1 = z1;
		this.x2 = x2;
		this.y2 = y2;
		this.z2 = z2;
		this.x3 = x3;
		this.y3 = y3;
		this.z3 = z3;
		
		a=new Vec3D(x1,y1,z1);
		b=new Vec3D(x2,y2,z2);
		c=new Vec3D(x3,y3,z3);
		init();
		// TODO Auto-generated constructor stub
	}

	public double getLength(double vx, double vy, double vz) {
		return (double) Math.sqrt(vx * vx + vy * vy + vz * vz);
	}

	public static double dot(double x1, double y1, double z1, double x2, double y2,
			double z2) {
		return x1 * x2 + y1 * y2 + z1 * z2;
	}

	 public double getAngle(double nx,double ny,double nz, double ax,double ay,double az) {
	        double dot = dot(nx,ny,nz,ax,ay,az);
	        double angle = Math.acos(dot);
	        return angle;
	    }
	
	 public void cross(double x1,double y1,double z1,double x2,double y2,double z2,double[]result){
		 result[0]=y1 * z2 - z1 * y2;
		 result[1] = z1 * x2 - x1 * z2;
		 result[2] = x1 * y2 - y1 * x2;
	 }
	public void init() {
		// calculate v12 & v13
		ux = x2 - x1;
		uy = y2 - y1;
		uz = z2 - z1;
		vx = x3 - x1;
		vy = y3 - y1;
		vz = z3 - z1;

		uu = dot(ux, uy, uz, ux, uy, uz);
		uv = dot(ux, uy, uz, vx, vy, vz);
		vv = dot(vx, vy, vz, vx, vy, vz);
		d = uv * uv - uu * vv;

		// calculate normal
		double[]normal=new double[3];
		cross(ux,uy,uz,vx,vy,vz,normal);
		nx = uy * vz - uz * vy;
		ny = uz * vx - ux * vz;
		nz = ux * vy - uy * vx;
		// normalize
		double mag = getLength(nx, ny, nz);  // could become very small
		//if (mag<0.00000001)System.out.println("very small normal");
		nx /= mag;
		ny /= mag;
		nz /= mag;
		if (Double.isNaN(nx)||Double.isNaN(ny)||Double.isNaN(nz)){
			System.out.println("nx: "+nx);
			System.out.println("ny: "+ny);
			System.out.println("nz: "+nz);
			System.out.println("vx: "+vx);
			System.out.println("vy: "+vy);
			System.out.println("vz: "+vz);
			System.out.println("ux: "+ux);
			System.out.println("uy: "+uy);
			System.out.println("uz: "+uz);
		}
		sd=dot(nx,ny,nz, nx,ny,nz);
		
		
		d0 = -dot(nx, ny, nz, x1, y1, z1);
		m = new PMatrix3D();


		double px = 0;
		double py = 0;
		double pz = 1;
		// if ()
		double angle = (double) Math.acos(dot(nx, ny, nz, px, py, pz));
		double uVx = ny * pz - nz * py;
		double uVy = nz * px - nx * pz;
		double uVz = nx * py - ny * px;

		// if same angle, u=0;
		// if (u.mag()<0.00001)System.out.println("small");


		double absAngle = Math.abs(angle);
		if (absAngle > 0.001f && absAngle < Math.PI-0.001f) {
			double magU = getLength(uVx, uVy, uVz);
			uVx /= magU;
			uVy /= magU;
			uVz /= magU;
			double c = (double) Math.cos(angle);
			double s = (double) Math.sin(angle);
			double t = 1f - c;
			m.m00 =(float)( c + uVx * uVx * t);
			m.m01 =(float)( uVx * uVy * t - uVz * s);
			m.m02 = (float)(uVx * uVz * t + uVy * s);
			m.m10 = (float)(uVy * uVx * t + uVz * s);
			m.m11 = (float)(c + uVy * uVy * t);
			m.m12 = (float)(uVy * uVz * t - uVx * s);
			m.m20 = (float)(uVz * uVx * t - uVy * s);
			m.m21 = (float)(uVz * uVy * t + uVx * s);
			m.m22 = (float)(c + uVz * uVz * t);
		} else {
			double dZ=Math.abs(z1-z2)+Math.abs(z3-z2)+Math.abs(z3-z1);
			
			if (dZ>0.0001){
				System.out.println("not in XY Plane: "+dZ);
				System.out.println("nx: "+nx);
				System.out.println("ny: "+ny);
				System.out.println("nz: "+nz);
			}
		}
		
		float[] f1 = new float[] {(float) x1, (float) y1,(float)  z1 };
		float[] f2 = new float[] {(float)  x2, (float) y2,(float)  z2 };
		float[] f3 = new float[] { (float) x3, (float) y3,(float)  z3 };

		float[] f1T = m.mult(f1, null);
		float[] f2T = m.mult(f2, null);
		float[] f3T = m.mult(f3, null);

		l1 = new MSegmentDouble(f1T[0], f1T[1], f1T[2], f2T[0], f2T[1], f2T[2]);
		l2 = new MSegmentDouble(f2T[0], f2T[1], f2T[2], f3T[0], f3T[1], f3T[2]);
		l3 = new MSegmentDouble(f3T[0], f3T[1], f3T[2], f1T[0], f1T[1], f1T[2]);
	}




	public double getDistanceExtrude(double x, double y, double z) {
		double minD = Double.MAX_VALUE;
		float[] pt = new float[] {(float) x,(float) y, (float)z };
		float[] trans = new float[3];

		trans = m.mult(pt, null);
		double cX = trans[0];
		double cY = trans[1];
		double cZ = trans[2];

		int r1 = Line2D.relativeCCW(l1.x1, l1.y1, l1.x2, l1.y2, cX, cY);
		int r2 = Line2D.relativeCCW(l2.x1, l2.y1, l2.x2, l2.y2, cX, cY);
		int r3 = Line2D.relativeCCW(l3.x1, l3.y1, l3.x2, l3.y2, cX, cY);
		double cDistance;
		if (r1 == r2 && r1 == r3) {
			cDistance = (cZ - l1.z2) * (cZ - l1.z2);
			return (double) Math.sqrt(cDistance);
		}
		return minD;
	}

	
	
	public double getDistanceSqr(double x, double y, double z) {
		double minD = Float.MAX_VALUE;
		float[] pt = new float[] { (float)x,(float) y, (float)z };
		
		// tranlate point
		float[] trans = new float[3];
		trans = m.mult(pt, null);
		double cX = trans[0];
		double cY = trans[1];
		double cZ = trans[2];

		
		// check if inside our outside
		int r1 = Line2D.relativeCCW(l1.x1, l1.y1, l1.x2, l1.y2, cX, cY);
		int r2 = Line2D.relativeCCW(l2.x1, l2.y1, l2.x2, l2.y2, cX, cY);
		int r3 = Line2D.relativeCCW(l3.x1, l3.y1, l3.x2, l3.y2, cX, cY);
		double cDistance;
		if (r1 == r2 && r1 == r3) {
			cDistance = (cZ - l1.z2) * (cZ - l1.z2);
			if (cDistance < minD) {
				minD = cDistance;
			}
		} 
		
		// find closest distance to line 
		else {
			double d1 = (double) l1.distToSegSqr(cX, cY, cZ);
			double d2 = (double) l2.distToSegSqr(cX, cY, cZ);
			double d3 = (double) l3.distToSegSqr(cX, cY, cZ);
			if (d1 < minD) {
				minD = d1;
			}
			if (d2 < minD) {
				minD = d2;
			}
			if (d3 < minD) {
				minD = d3;
			}
		}

		return (double) minD;
	}

	
	public double getDistance(double x, double y, double z) {
		// TODO Auto-generated method stub
		return (double) Math.sqrt(getDistanceSqr(x, y, z));
	}

	
	// not sure if this is right
	public double getDistancePlane(double x, double y, double z) {
		return d0 + dot(nx, ny, nz, x, y, z);

	}
	
	//http://geomalgorithms.com/a04-_planes.html
	public double getDistanceToPlane(double x,double y,double z){
		double    sb, sn;
	    sn = -dot( nx,ny,nz, x-x1,y-y1,y-z1);
	    sb = sn / sd;
	    double rX=sb*nx;
	    double rY=sb*ny;
	    double rZ=sb*nz;
	    return getLength(rX,rY,rZ);
	}

	public double getDistanceClosestPoint(double x, double y, double z) {

		// TODO Auto-generated method stub
		Vec3D p=new Vec3D(x,y,z);
		Vec3D cP=closestPoint(new Vec3D(x,y,z));
		return getLength(cP.x-p.x,cP.y-p.y,cP.z-p.z);
	}
	 public Vec3D closestPoint(Vec3D p) {
         Vec3D ab = b.getSub(a);
         Vec3D ac = c.getSub(a);
         Vec3D bc = c.getSub(b);

         Vec3D pa = p.getSub(a);
         Vec3D pb = p.getSub(b);
         Vec3D pc = p.getSub(c);

         Vec3D ap = a.getSub(p);
         Vec3D bp = b.getSub(p);
         Vec3D cp = c.getSub(p);

         // Compute parametric position s for projection P' of P on AB,
         // P' = A + s*AB, s = snom/(snom+sdenom)
         double snom = pa.dot(ab);
         double sdenom = pb.dot(a.getSub(b));

         // Compute parametric position t for projection P' of P on AC,
         // P' = A + t*AC, s = tnom/(tnom+tdenom)
         double tnom = pa.dot(ac);
         double tdenom = pc.dot(a.getSub(c));

         if (snom <= 0.0f && tnom <= 0.0f)
                 return a; // Vertex region early out

         // Compute parametric position u for projection P' of P on BC,
         // P' = B + u*BC, u = unom/(unom+udenom)
         double unom = pb.dot(bc);
         double udenom = pc.dot(b.getSub(c));

         if (sdenom <= 0.0f && unom <= 0.0f)
                 return b; // Vertex region early out
         if (tdenom <= 0.0f && udenom <= 0.0f)
                 return c; // Vertex region early out

         // P is outside (or on) AB if the triple scalar product [N PA PB] <= 0
         Vec3D n = ab.cross(ac);
         double vc = n.dot(ap.crossSelf(bp));

         // If P outside AB and within feature region of AB,
         // return projection of P onto AB
         if (vc <= 0.0f && snom >= 0.0f && sdenom >= 0.0f) {
                 // return a + snom / (snom + sdenom) * ab;
                 return a.getAdd(ab.scale((float)(snom / (snom + sdenom))));
         }

         // P is outside (or on) BC if the triple scalar product [N PB PC] <= 0
         double va = n.dot(bp.crossSelf(cp));
         // If P outside BC and within feature region of BC,
         // return projection of P onto BC
         if (va <= 0.0f && unom >= 0.0f && udenom >= 0.0f) {
                 // return b + unom / (unom + udenom) * bc;
                 return b.getAdd(bc.scale((float)(unom / (unom + udenom))));
         }

         // P is outside (or on) CA if the triple scalar product [N PC PA] <= 0
         double vb = n.dot(cp.crossSelf(ap));
         // If P outside CA and within feature region of CA,
         // return projection of P onto CA
         if (vb <= 0.0f && tnom >= 0.0f && tdenom >= 0.0f) {
                 // return a + tnom / (tnom + tdenom) * ac;
                 return a.getAdd(ac.scale((float)(tnom / (tnom + tdenom))));
         }

         // P must project inside face region. Compute Q using barycentric
         // coordinates
         double u = va / (va + vb + vc);
         double v = vb / (va + vb + vc);
         double w = 1.0f - u - v; // = vc / (va + vb + vc)
         // return u * a + v * b + w * c;
         return a.getScaled((float)u).add((b.scale((float)v)).add(c.scale((float)w)));
 }

	public void calculateBounds(double[] bounds) {
		bounds[0] = x1;
		bounds[1] = y1;
		bounds[2] = z1;
		bounds[3] = x1;
		bounds[4] = y1;
		bounds[5] = z1;

		bounds[0] = Math.min(bounds[0], x2);
		bounds[0] = Math.min(bounds[0], x3);
		bounds[1] = Math.min(bounds[1], y2);
		bounds[1] = Math.min(bounds[1], y3);
		bounds[2] = Math.min(bounds[2], z2);
		bounds[2] = Math.min(bounds[2], z3);

		bounds[3] = Math.max(bounds[3], x2);
		bounds[3] = Math.max(bounds[3], x3);
		bounds[4] = Math.max(bounds[4], y2);
		bounds[4] = Math.max(bounds[4], y3);
		bounds[5] = Math.max(bounds[5], z2);
		bounds[5] = Math.max(bounds[5], z3);
	}
	
	public int triIntersect(double px, double py, double pz, double dx, double dy,
			double dz, double[] intersection) {

		double w0x = px - x1;
		double w0y = py - y1;
		double w0z = pz - z1;

		double a = -dot(nx, ny, nz, w0x, w0y, w0z);
		double b = dot(nx, ny, nz, dx, dy, dz);

		if (Math.abs(b) < SMALL_NUM) { // ray is parallel to triangle plane
			if (a == 0) // ray lies in triangle plane
				return SAME_PLANE;
			else
				return DISJOINT; // ray disjoint from plane
		}

		double r = a / b;
		if (r < 0.0) // ray goes away from triangle
			return DISJOINT; // => no intersect

		intersection[0] = px + r * dx;
		intersection[1] = py + r * dy;
		intersection[2] = pz + r * dz;

		double wx = intersection[0] - x1;
		double wy = intersection[1] - y1;
		double wz = intersection[2] - z1;

		double wu = dot(wx, wy, wz, ux, uy, uz);
		double wv = dot(wx, wy, wz, vx, vy, vz);

		// get and test parametric coords

		double s = (uv * wv - vv * wu) / d;
		if (s < 0.0 || s > 1.0) // I is outside T
			return DISJOINT;
		double t = (uv * wu - uu * wv) / d;
		if (t < 0.0 || (s + t) > 1.0) // I is outside T
			return DISJOINT;
		return INTERSECT; // I is in T
	}

	public int triIntersect(double px, double py, double pz, double dx, double dy,
			double dz, double x1, double y1, double z1, double x2, double y2,
			double z2, double x3, double y3, double z3, double[] intersection) {
		double ux = x2 - x1;
		double uy = y2 - y1;
		double uz = z2 - z1;
		double vx = x3 - x1;
		double vy = y3 - y1;
		double vz = z3 - z1;

		double nx = uy * vz - uz * vy;
		double ny = uz * vx - ux * vz;
		double nz = ux * vy - uy * vx;

		double w0x = px - x1;
		double w0y = py - y1;
		double w0z = pz - z1;

		double a = -dot(nx, ny, nz, w0x, w0y, w0z);
		double b = dot(nx, ny, nz, dx, dy, dz);

		if (Math.abs(b) < SMALL_NUM) { // ray is parallel to triangle plane
			if (a == 0) // ray lies in triangle plane
				return SAME_PLANE;
			else
				return DISJOINT; // ray disjoint from plane
		}

		double r = a / b;
		if (r < 0.0) // ray goes away from triangle
			return DISJOINT; // => no intersect

		intersection[0] = px + r * dx;
		intersection[1] = py + r * dy;
		intersection[2] = pz + r * dz;

		double uu = dot(ux, uy, uz, ux, uy, uz);
		double uv = dot(ux, uy, uz, vx, vy, vz);
		double vv = dot(vx, vy, vz, vx, vy, vz);

		double wx = intersection[0] - x1;
		double wy = intersection[1] - y1;
		double wz = intersection[2] - z1;

		double wu = dot(wx, wy, wz, ux, uy, uz);
		double wv = dot(wx, wy, wz, vx, vy, vz);
		double d = uv * uv - uu * vv;

		// get and test parametric coords

		double s = (uv * wv - vv * wu) / d;
		if (s < 0.0 || s > 1.0) // I is outside T
			return DISJOINT;
		double t = (uv * wu - uu * wv) / d;
		if (t < 0.0 || (s + t) > 1.0) // I is outside T
			return DISJOINT;
		return INTERSECT; // I is in T
	}

	public int triIntersect2(double px, double py, double pz, double dx, double dy,
			double dz, double x1, double y1, double z1, double x2, double y2,
			double z2, double x3, double y3, double z3, double[] intersection) {
		double e1x = x2 - x1;
		double e1y = y2 - y1;
		double e1z = z2 - z1;
		double e2x = x3 - x1;
		double e2y = y3 - y1;
		double e2z = z3 - z1;

		double pVecX = dy * e2z - dz * e2y;
		double pVecY = dz * e2x - dx * e2z;
		double pVecZ = dx * e2y - dy * e2x;

		double det = dot(e1x, e1y, e1z, pVecX, pVecY, pVecZ);
		if (det > -SMALL_NUM && det < SMALL_NUM)
			return DISJOINT;
		double inv_det = 1f / det;

		double tVecX = px - x1;
		double tVecY = py - y1;
		double tVecZ = pz - z1;

		double u = dot(tVecX, tVecY, tVecZ, pVecX, pVecY, pVecZ) * inv_det;

		if (u < 0 || u > 1)
			return DISJOINT;

		double qVecX = tVecY * e1z - tVecZ * e1y;
		double qVecY = tVecZ * e1x - tVecX * e1z;
		double qVecZ = tVecX * e1y - tVecY * e1x;

		double v = dot(dx, dy, dz, qVecX, qVecY, qVecZ) * inv_det;
		if (v < 0.0 || u + v > 1.0)
			return DISJOINT;
		double t = dot(e2x, e2y, e2z, qVecX, qVecY, qVecZ) * inv_det;
		intersection[0] = px + t * dx;
		intersection[1] = py + t * dy;
		intersection[2] = pz + t * dz;
		return INTERSECT;
	}

	public double ptSegDistSq(double[] seg, double x, double y, double z) {
		double vx = x2 - x1;
		double vy = y2 - y1;
		double vz = z2 - z1;
		double c2 = dot(vx, vy, vz, vx, vy, vz);
		double dx = x - x1;
		double dy = y - y1;
		double dz = z - z1;
		double c1 = dot(dx, dy, dz, vx, vy, vz);
		if (c1 <= 0)
			return distSqr(x, y, z, x1, y1, z1);
		if (c2 <= c1)
			return distSqr(x, y, z, x2, y2, z2);
		double b = c1 / c2;
		double px = x1 + b * vx;
		double py = y1 + b * vy;
		double pz = z1 + b * vz;
		return distSqr(x, y, z, px, py, pz);
	}

	public double distSqr(double x1, double y1, double z1, double x2, double y2,
			double z2) {
		double dX = x2 - x1;
		double dY = y2 - y1;
		double dZ = z2 - z1;
		return dX * dX + dY * dY + dZ * dZ;
	}

}
